.data
index:		.word 0
state:		.word 0
sorted_index:	.word -1
new_angle:  .word 0
abs_diff:   .word 0

# syscall constants
PRINT_STRING = 4

# spimbot constants
VELOCITY = 0xffff0010
ANGLE = 0xffff0014
ANGLE_CONTROL = 0xffff0018
BOT_X = 0xffff0020
BOT_Y = 0xffff0024

TIMER = 0xffff001c
TIMER_MASK = 0x8000
TIMER_ACKNOWLEDGE = 0xffff006c

.text

# iteratePath ########################################################
#
# arguments: $a0:    spimbot x
#            $a1:    spimbot y
#            $a2:    x_coords[]
#            $a3:    y_coords[]
#            0($sp): array of distances (to be modified)
#            4($sp): size of array(s)

.globl iteratePath
iteratePath:
    
    li   $t0, 0                 # $t0 hold 0, i.e. i
    lw   $t1, 0($sp)            # $t1 holds the address of array_distances
    lw   $t2, 4($sp)            # $t2 holds the value of array_size
    sub  $sp, $sp, 36           # set up stack frame
    sw   $ra, 8($sp)            # store on the stack
    
    li   $t6, TIMER_MASK        # enable timer interrupt
    or   $t6, $t6, 1            # enabled global interrupt
    mtc0 $t6, $12               # interrupt mask set

    lw   $t8, TIMER             # get_time()
    add  $t8, $t8, 100          # add 100 to get_time()
    sw   $t8, TIMER             # set_timer(get_time() + 100)

loop:
    bge  $t0, $t2, loop_done    # loop conditional
    sw   $a0, 12($sp)           # store on the stack
    sw   $a1, 16($sp)           # ...
    sw   $a2, 20($sp)           # ...
    sw   $a3, 24($sp)           # ...
    sw   $t0, 28($sp)           # ...
    sw   $t2, 32($sp)           # ...

    sll  $t3, $t0, 2            # $t3 holds i * 4 for the arrays
    add  $a2, $a2, $t3          # $a2 holds &x_coords[i]
    add  $a3, $a3, $t3          # $a3 holds &y_coords[i]
    add  $t1, $t1, $t3          # $t1 holds &distances[i]
    sub  $t7, $t2, $t0          # $t7 holds array_size - i

    sw   $t1, 0($sp)            # store $t1 on the stack for calculatePath
    sw   $t7, 4($sp)            # store $t7 on the stack for calculatePath
    
    jal  calculatePath          # call to calculatePath

    lw   $a0, 12($sp)           # restore from stack
    lw   $a1, 16($sp)           # ...
    lw   $a2, 20($sp)           # ...
    lw   $a3, 24($sp)           # ...
    lw   $t0, 28($sp)           # ...
    lw   $t2, 32($sp)           # ...
    lw   $t1, 0($sp)            # ...

    la   $t9, sorted_index      # $t9 holds the address of global variable sorted_index
    sw   $t0, 0($t9)            # assign sorted_index the value of i
    
    sll  $t3, $t0, 2            # $t3 holds i * 4 
    add  $t4, $a2, $t3          # $t4 holds the &x_coords[i]
    add  $t5, $a3, $t3          # $t5 holds the &y_coords[i]

    lw   $a0, 0($t4)            # assign x1 = x_coords[i]
    lw   $a1, 0($t5)            # assign y1 = y_coords[i]

    add  $t0, $t0, 1            # increment i by 1
    j    loop                   # back to the loop

loop_done:
    
    lw   $ra, 8($sp)            # restore $ra
    add  $sp, $sp, 36           # clear stack space
    jr	 $ra                    # return to caller

.kdata
save0:	.word 0
save1:	.word 0
chunkIH: .space 32              # space for 8 registers
non_intrpt_str:    .asciiz "Non-interrupt exception\n"
unhandled_str:    .asciiz "Unhandled interrupt type\n"

.ktext 0x80000180
interrupt_handler:
	.set noat
	move	$k1, $at		# Save $at
	.set at
	sw	$a0, save0		# Get some free registers
	sw	$a1, save1		# by storing them to global variables

	mfc0 	$k0, $13		# Get Cause register
	srl 	$a0, $k0, 2		
	and 	$a0, $a0, 0xf		# ExcCode field
	bne 	$a0, 0, non_intrpt

interrupt_dispatch:			# Interrupt:
	mfc0 	$k0, $13		# Get Cause register, again
	beq	$k0, 0, done		# handled all outstanding interrupts

    and     $a0, $k0, 0x8000         # is there a timer interrupt?
    bne     $a0, 0, timer_interrupt 

	li	$v0, PRINT_STRING	# Unhandled interrupt types
	la	$a0, unhandled_str
	syscall
	j	done

non_intrpt:				# was some non-interrupt
	li	$v0, PRINT_STRING			
	la	$a0, non_intrpt_str
	syscall				# print out an error message
	j	done

done:
	lw	$a0, save0
	lw	$a1, save1
	.set noat
	move	$at, $k1		# Restore $at
	.set at
	eret

timer_interrupt:
    sw      $a1, TIMER_ACKNOWLEDGE          # acknoledge interrupt

    la      $k0, chunkIH                    # get chunkIH address
    sw      $t0, 0($k0)                     # store on chunkIH
    sw      $t1, 4($k0)                     # ...
    sw      $t2, 8($k0)                     # ...
    sw      $t3, 12($k0)                    # ...
    sw      $t4, 16($k0)                    # ...
    sw      $t5, 20($k0)                    # ...
    sw      $t6, 24($k0)                    # ...
    sw      $t7, 28($k0)                    # ...

infinite:
    la      $t7, sorted_index               # $t7 holds the address of sorted_index
    la      $t6, index                      # $t6 holds the address of index

    lw      $t7, 0($t7)                     # $t7 holds the value of sorted_index
    lw      $t6, 0($t6)                     # $t6 holds the value of index

    sll     $t5, $t6, 2                     # $t5 = index * 4
    
    bge     $t7, $t6, if_2                  # if condition (sorted_index < index)
    sw      $zero, VELOCITY                 # set_velocity(0)
    lw      $t4, TIMER                      # get_timer()
    add     $t4, $t4, 400                   # get_timer() + 400
    sw      $t4, TIMER                      # set_timer(get_timer() + 400)

    lw      $t0, 0($k0)                     # restore from chunkIH
    lw      $t1, 4($k0)                     # ...
    lw      $t2, 8($k0)                     # ...
    lw      $t3, 12($k0)                    # ...
    lw      $t4, 16($k0)                    # ...
    lw      $t5, 20($k0)                    # ...
    lw      $t6, 24($k0)                    # ...
    lw      $t7, 28($k0)                    # ...

    j     interrupt_dispatch          

if_2:
    
    lw      $t4, state                      # $t4 holds the value of state
    bne     $t4, $0, else_case              # if conditional (state == 0)
    lw      $t0, BOT_X                      # $t0 = current_x  = get_current_x()
    la      $t1, X                          # $t1 has the address of X
    add     $t1, $t1, $t5                   # $t1 has the address of x_coords[index]
    lw      $t1, 0($t1)                     # $t1 = target_x = x_coords[index]
    sub     $t1, $t1, $t0                   # $t1 = diff = target_x - current_x
    abs     $t2, $t1                        # $t2 = abs_diff = abs(diff) 
    sw      $t2, abs_diff                   # assign abs_diff 

    li      $t3, 2

    bge     $t2, $t3, else_in_first_if      # if conditional (abs_diff < 2)
    li      $t4, 1                  
    sw      $t4, state                      # assign state = 1
    j       infinite                        # continue statement

else_in_first_if:
        
    ble     $t1, $0, else_in_else_in_first_if
    sw      $0, new_angle                   # assign new_angle = 0
    j       infinite_end                    # break

else_in_else_in_first_if:

    li      $t4, 180                        # $t4 = 180 for new_angle
    sw      $t4, new_angle                  # assign new_angle = 180
    j       infinite_end

else_case:

    lw      $t0, BOT_Y                      # $t0 = current_y = get_current_y()
    la      $t1, Y                          # $t1 has address of Y
    add     $t1, $t1, $t5                   # $t1 has address of y_coord[index]                   
    lw      $t1, 0($t1)                     # $t1 has the value of y_coord[index]
    sub     $t1, $t1, $t0                   # $t1 = diff = target_y - current_y
    abs     $t2, $t1                        # $t2 = abs_diff = abs(diff)
    sw      $t2, abs_diff                   # assign abs_diff

    li      $t3, 2                          # $t3 = 2

    bge     $t2, $t3, else_in_else_case     # if conditional (abs_diff < 2)
    sw      $0, state                       # assign state = 0
    add     $t6, $t6, 1                     # index = index + 1                     
    sw      $t6, index                      # assign index the new value
    j       infinite

else_in_else_case:
    
    ble     $t1, $0, else_in_else_in_else_case
    li      $t4, 90                         # $t4 holds value 90
    sw      $t4, new_angle                  # assign new_angle = 90
    j       infinite_end

else_in_else_in_else_case:

    li      $t4, 270                        # $t4 holds the value 270
    sw      $t4, new_angle                  # assign new_angle = 270
    j       infinite_end

infinite_end:

	lw		$t4, new_angle                 # $t4 holds the value of new_angle
    sw      $t4, ANGLE                     # store the value of new_angle to ANGLE
    li      $t1, 1                         # load 1 in $t1
    sw      $t1, ANGLE_CONTROL             # set ANGLE_CONTROL to 1 for absolute angle
    li      $t1, 10                        # load 10 in $t1 for velocity
    sw      $t1, VELOCITY                  # set VELOCITY to 10
    lw      $t2, TIMER                     # load timer in $t2
    lw      $t3, abs_diff                  # load value of abs_diff in $t3
    mul     $t3, $t3, 400                  # multiply abs_diff * 400 
    add     $t2, $t2, $t3                  # get_timer() + 400 * abs_diff
    sw      $t2, TIMER                     # call to set_timer()

    lw      $t0, 0($k0)                    # restore from chunkIH
    lw      $t1, 4($k0)                    # ...
    lw      $t2, 8($k0)                    # ...
    lw      $t3, 12($k0)                   # ...
    lw      $t4, 16($k0)                   # ...
    lw      $t5, 20($k0)                   # ...
    lw      $t6, 24($k0)                   # ...
    lw      $t7, 28($k0)                   # ...

    j       interrupt_dispatch
